/** ***********************************************************************************
* @file         rsa_driver.c
* @brief        rsa1024操作部分
* @details      
* @author       
* @date         2021/3
* @copyright    北京高知科技有限公司    
* @version      V1.0
* @attention    无
**************************************************************************************/

#define OSI_LOG_TAG OSI_MAKE_LOG_TAG('M', 'B', 'L', 'E')

#include "fibo_opencpu.h"
#include "stdio.h"
#include "stdlib.h"
#include "string.h"

#include "rsa_driver.h"
#include "tmath.h"
#include "bluetooth_app.h"



//关于base64的转换
//https://en.wikipedia.org/wiki/Base64
const unsigned char base64Table[64] = {
	'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
	'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
	'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
	'w', 'x', 'y', 'z', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '+', '/',
};


/*
下面这个转换，以后应该改成数组
尝试直接使用
base64_dec_map
*/
unsigned char base64TableRev(unsigned char cLetter)
{
	unsigned char reDa;

	if ((cLetter >= 'A') && (cLetter <= 'Z'))
	{
		reDa = cLetter - 'A';
	}
	else if ((cLetter >= 'a') && (cLetter <= 'z'))
	{
		reDa = cLetter - 'a' + 26;
	}
	else if ((cLetter >= '0') && (cLetter <= '9'))
	{
		reDa = cLetter - '0' + 26 + 26;
	}
	else if (cLetter == '+')
	{
		reDa = 62;
	}
	else if (cLetter == '/')
	{
		reDa = 63;
	}
	else
	{
		//error
		reDa = 0;
	}

	return reDa;

}

/*
返回0成功，1失败,格式错误
*/
unsigned char base64Decode(unsigned char * inStr, UINT16 inLen,
	unsigned char * outStr, UINT16 * outLen)
{
	unsigned char b1, b2, b3, b4;
	unsigned int i, j, NewStrCnt;
	//unsigned int ulTmp;

	*outLen = 0;

	if ((inLen % 4) != 0)
	{
		return 1;
	}

	if (inLen < 4)
	{
		return 1;
	}

	NewStrCnt = (inLen / 4) * 3;


	if (inStr[inLen - 1] == '=')
	{
		--NewStrCnt;
		if (inStr[inLen - 2] == '=')
		{
			--NewStrCnt;
		}
	}

	*outLen = NewStrCnt;

	for (i = 0, j = 0; i < NewStrCnt;)
	{
		b1 = base64TableRev(inStr[j++]);
		b2 = base64TableRev(inStr[j++]);
		b3 = base64TableRev(inStr[j++]);
		b4 = base64TableRev(inStr[j++]);


		outStr[i++] = ((b1 << 2) | (b2 >> 4));
		outStr[i++] = ((b2 << 4) | (b3 >> 2));
		outStr[i++] = ((b3 << 6) | b4);

	}

	return 0;

}



/*
返回0成功，1失败,格式错误
*/
unsigned char base64Encode(unsigned char * inStr, unsigned int inLen,
	unsigned char * outStr, unsigned int * outLen)
{
	unsigned int i, j, In3Cnt, leftN;
	//unsigned int ulTmp;

	leftN = inLen % 3;
	In3Cnt = inLen - leftN;

	for (i = 0, j = 0; i < In3Cnt; i += 3)
	{

		outStr[j++] = (inStr[i] & 0xFC) >> 2;
		outStr[j++] = ((inStr[i] & 0x03) << 4) | ((inStr[i + 1] & 0xF0) >> 4);
		outStr[j++] = ((inStr[i + 1] & 0x0F) << 2) | ((inStr[i + 2] & 0xC0) >> 6);
		outStr[j++] = inStr[i + 2] & 0x3F;
	}

	for (i = 0; i < j; i++)
	{
		outStr[i] = base64Table[outStr[i] & 0x3F];
	}

	i = inLen - leftN;
	if (leftN == 1)
	{
		outStr[j] = (inStr[i] & 0xFC) >> 2;
		outStr[j] = base64Table[outStr[j] & 0x3F];
		j++;

		outStr[j] = ((inStr[i] & 0x03) << 4);
		outStr[j] = base64Table[outStr[j] & 0x3F];
		j++;
		outStr[j++] = '=';
		outStr[j++] = '=';
	}
	else if (leftN == 2)
	{
		outStr[j] = (inStr[i] & 0xFC) >> 2;
		outStr[j] = base64Table[outStr[j] & 0x3F];
		j++;

		outStr[j] = ((inStr[i] & 0x03) << 4) | ((inStr[i + 1] & 0xF0) >> 4);
		outStr[j] = base64Table[outStr[j] & 0x3F];
		j++;

		outStr[j] = ((inStr[i + 1] & 0x0F) << 2);
		outStr[j] = base64Table[outStr[j] & 0x3F];
		j++;

		outStr[j++] = '=';
	}

	*outLen = j;

	return 0;
}

/*
去掉pem文件的头和尾，和无关的字节，比如回车换行等
*/
void GetRSAKeyFromPem(UINT8 * pDa, UINT16 * len)
{
	UINT8 FindStep;
	UINT16 iLen;
	UINT16 i, j;

	iLen = *len;

	if (iLen < 10)
	{
		return;
	}

	FindStep = 0;
	j = 0;
	for (i = 0; i < iLen; i++)
	{
		//发现回车换行，去掉
		if ((pDa[i] == 0x0A) ||
			(pDa[i] == 0x0D) ||
			(pDa[i] == 0x09) ||			//水平制表符
			(pDa[i] == ' ')) //关于空格，这个一般是测试时引入的，删除无妨
		{
			continue;
		}

		//去掉开头和结尾的----- xxx -----
		switch (FindStep)
		{
		case 0:
			if (pDa[i] == '-')
			{
				FindStep++;
			}
			break;

		case 1:
			if (pDa[i] != '-')
			{
				FindStep++;
			}
			break;

		case 2:
			if (pDa[i] == '-')
			{
				FindStep++;
			}
			break;

		case 3:
			if (pDa[i] != '-')
			{
				FindStep++;
				pDa[j++] = pDa[i];
			}
			break;

		case 4:
			if (pDa[i] != '-')
			{
				pDa[j++] = pDa[i];
			}
			else if (pDa[i] == '-')
			{
				FindStep++;
			}
			break;

		case 5:
			i = iLen;
			break;
		}

	}

	//说明传过来的报文，没有---xxx--- 这些信息
	if (FindStep == 0)
	{
		j = 0;
		for (i = 0; i < iLen; i++)
		{
			//发现回车换行，去掉
			if ((pDa[i] == 0x0A) ||
				(pDa[i] == 0x0D))
			{
				continue;
			}
			pDa[j++] = pDa[i];
		}
	}

	*len = j;

}


/*
//蓝牙串口接收解析；
pCipheredData：待解析的密文
len：密文长度
pPubKey：公钥：
pubKeyLen：公钥长度
outDa：固定输出的就是28字节
返回值
返回0代表成功，其他值代表错误。
*/
UINT32 BLEanalyRSA(UINT8 *pCipheredData, UINT16 len, UINT8 *pPubKey, UINT16 pubKeyLen, UINT8 * outDa)
{
		/*const char * pemKey = "-----BEGIN PUBLIC KEY----- \
			MIGfMA0GCSqGSIb3DQEBAQUAA4GNADCBiQKBgQCrRn2yI4DgB6K2EkyW2PeFiM6q \
			5St+X5bE8GWf7SwNWEpwUG7oBDCwCVu/ehIl5hzkXuV+WOppKAJ8xqZqKqZN45u+ \
			3TEl17ul0ogf78Hsj0ztYqeC4bZrrvNaTFTxFGGiO4X5FFeY7cCAd2r1Df2ryA5u \
			WcjTPDrxJf5lfEbbMwIDAQAB \
			-----END PUBLIC KEY-----";*/
	/*
	30 81 9F
	30 0D 06 09 2A 86 48 86 F7 0D
	01 01 01 05 00
	03 81 8D 00
	30 81 89
	02 81 81 00

	AB 46 7D B2 23 80 E0 07 A2 B6 12 4C 96 D8 F7 85
	88 CE AA E5 2B 7E 5F 96 C4 F0 65 9F ED 2C 0D 58
	4A 70 50 6E E8 04 30 B0 09 5B BF 7A 12 25 E6 1C
	E4 5E E5 7E 58 EA 69 28 02 7C C6 A6 6A 2A A6 4D
	E3 9B BE DD 31 25 D7 BB A5 D2 88 1F EF C1 EC 8F
	4C ED 62 A7 82 E1 B6 6B AE F3 5A 4C 54 F1 14 61
	A2 3B 85 F9 14 57 98 ED C0 80 77 6A F5 0D FD AB
	C8 0E 6E 59 C8 D3 3C 3A F1 25 FE 65 7C 46 DB 33

	02 03
	01 00 01
	*/
	//	const char * cInputStr = "DCuegisrzNPQzXr6uUP8l6djWZ2CqV86bjnpsTWNN0PJDME/OY9O2QxSkcohP8UdOmw51BnqcCEMQgG0G8hZM7gcNlsTQTWNDjEHdkP/2ZlufD59A9/96lnRpIwATXhbe4donVlfnYKg94U5W4944XwR90kM5LtXFdmYPOPbYZ8=";
	//	const char * cInputStr = "oJcfkkiAlQLj7Za7fFZizXFxy/Xf0NYH+4iNxRRND5t/l7VH7RemibQkrHj1YyuqLORx2/FicbWi0n3FGGeY9Y8joCLuQyPt8PYpbYtBVMDI/hduDTusYCt+c90OvZHFtL3aFWwi6RmHTNlDL+Vf6ft90eQHoTW2D8dvYPLoy5Q=";

	unsigned char pubKey[300];
	//UINT16 pubKeyLen;

	unsigned char e_d[3];
	//unsigned char e_len;

	unsigned char InStr[300];
	UINT16 InStrLen;

	unsigned char pubOutStr[300];
	unsigned int  pubOutStrLen;

	unsigned int reDa;
	unsigned int i;



	//实际基本长度固定，原始包272字节
	if (pubKeyLen > 300)
	{
		return 1;
	}

	
	memcpy(pubKey, pPubKey, pubKeyLen);
	GetRSAKeyFromPem(pubKey, &pubKeyLen);

	//理论上服务器下发的秘钥是1024位，是经过base64加密的，去掉头尾后，应该是216
	if (pubKeyLen != 216)
	{
		if (pubKeyLen == 0)
		{
			send_rsa_error(4);
		}
		else{
			send_rsa_error(6);
		}
		return 2;
	}

	if (base64Decode(pubKey, pubKeyLen, pubKey, &pubKeyLen) != 0)
	{
		send_rsa_error(7);
		return 3;
	}

	if (pubKeyLen != 162)
	{
		send_rsa_error(8);
		return 4;
	}

	//取出真正的秘钥hex
	memcpy(pubKey, &pubKey[29], 128);
	pubKeyLen = 128;

	//取出e
	memcpy(e_d, &pubKey[29 + 128 + 2], 3);
	//e_len = 3;

	//把密文数据先经过base64解密，之后才能调用rsa公钥解密	
	InStrLen = len;
	memcpy(InStr, pCipheredData, InStrLen);

	if (base64Decode(InStr, InStrLen, InStr, &InStrLen) != 0)
	{
		send_rsa_error(9);
		return 5;
	}

	//秘钥是1024，解密的密文也是1024位
	if (InStrLen != 128)
	{
		send_rsa_error(0x10);
		return 6;
	}

	//下面开始调用rsa 公钥解密函数
	if (RSA1024bitCalc(pubKey, e_d, InStr, pubOutStr) == 0)
	{
		//解密成功
		i = i;
	}

	//pkcs 找出有效数据
	reDa = 0;
	pubOutStrLen = 128;
	pkcs_1_v1_5_decode(pubOutStr, 128, LTC_LTC_PKCS_1_EMSA, 1024, pubOutStr, &pubOutStrLen, &reDa);

	memcpy(outDa, pubOutStr, 28);
	
	return 0;
}





/*
n: 模, 秘钥
e: 公钥指数
c: 待验证的私钥加密的密文
t: 明文，输出报文

*/
int RSA1024bitCalc(unsigned char *n, unsigned char *e, unsigned char *c, unsigned char *t)
{
	int err;
	mp_int mp_n, mp_e, mp_c, mp_t, mp_y;//大数类型声明

	err = 0;

	mp_init_multi(&mp_n);//初始化
	mp_init_multi(&mp_e);//初始化
	mp_init_multi(&mp_c);//初始化
	mp_init_multi(&mp_y);//初始化


	mp_read_unsigned_bin(&mp_n, n, 128);//从字符数组中读取大数
	mp_read_unsigned_bin(&mp_e, e, 3);
	mp_read_unsigned_bin(&mp_c, c, 128);
	mp_read_unsigned_bin(&mp_t, t, 128);


	mp_exptmod_fast0(&mp_c, &mp_e, &mp_n, &mp_y);//幂模运算


	mp_to_unsigned_bin(&mp_y, t);

	if (mp_y.used == 0)//若幂模运算结果为0
	{
		err = -2;
		return err;
	}

	mp_clear_multi(&mp_n);//清理大数内存
	mp_clear_multi(&mp_e);//清理大数内存
	mp_clear_multi(&mp_t);//清理大数内存
	mp_clear_multi(&mp_y);//清理大数内存
	//mp_clear_multi(&mp_n, &mp_e, &mp_c, &mp_t, &mp_y, NULL);//清理大数内存

	return err;
}






/** @brief LTC_PKCS #1 v1.5 decode.
*
*  @param msg              The encoded data to decode
*  @param msglen           The length of the encoded data (octets)
*  @param block_type       Block type to use in padding (\sa ltc_pkcs_1_v1_5_blocks)
*  @param modulus_bitlen   The bit length of the RSA modulus
*  @param out              [out] Destination of decoding
*  @param outlen           [in/out] The max size and resulting size of the decoding
*  @param is_valid         [out] Boolean whether the padding was valid  1代表成功
*
*  @return CRYPT_OK if successful (even if invalid),
*/
int pkcs_1_v1_5_decode(const unsigned char *msg,
	unsigned int  msglen,
	unsigned int  block_type,
	unsigned int  modulus_bitlen,
	unsigned char *out,
	unsigned int *outlen,
	unsigned int *is_valid)
{
	unsigned int modulus_len, ps_len, i;
	int result;

	/* default to invalid packet */
	*is_valid = 0;

	modulus_len = (modulus_bitlen >> 3) + (modulus_bitlen & 7 ? 1 : 0);

	/* test message size */

	if ((msglen > modulus_len) || (modulus_len < 11)) {
		return CRYPT_PK_INVALID_SIZE;
	}

	/* separate encoded message */
	if ((msg[0] != 0x00) || (msg[1] != (unsigned char)block_type)) {
		result = CRYPT_INVALID_PACKET;
		goto bail;
	}


	if (block_type == LTC_LTC_PKCS_1_EME) {
		for (i = 2; i < modulus_len; i++) {
			/* separator */
			if (msg[i] == 0x00) { break; }
		}
		ps_len = i++ - 2;

		if ((i >= modulus_len) || (ps_len < 8)) {
			/* There was no octet with hexadecimal value 0x00 to separate ps from m,
			* or the length of ps is less than 8 octets.
			*/
			result = CRYPT_INVALID_PACKET;
			goto bail;
		}
	}
	else {
		for (i = 2; i < modulus_len - 1; i++) {
			if (msg[i] != 0xFF) { break; }
		}

		/* separator check */
		if (msg[i] != 0) {
			/* There was no octet with hexadecimal value 0x00 to separate ps from m. */
			result = CRYPT_INVALID_PACKET;
			goto bail;
		}

		ps_len = i - 2;
	}

	if (*outlen < (msglen - (2 + ps_len + 1))) {
		*outlen = msglen - (2 + ps_len + 1);
		result = CRYPT_BUFFER_OVERFLOW;
		goto bail;
	}

	*outlen = (msglen - (2 + ps_len + 1));
	memcpy(out, &msg[2 + ps_len + 1], *outlen);

	/* valid packet */
	*is_valid = 1;
	result = CRYPT_OK;
bail:
	return result;
} /* pkcs_1_v1_5_decode */


/*
Test if a PRNG index is valid
@param idx   The index of the PRNG to search for
@return CRYPT_OK if valid
*/
int prng_is_valid(int idx)
{
	//   LTC_MUTEX_LOCK(&ltc_prng_mutex);
	//   if (idx < 0 || idx >= TAB_SIZE || prng_descriptor[idx].name == NULL) {
	//      LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
	//      return CRYPT_INVALID_PRNG;
	//   }
	//   LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
	return CRYPT_OK;
}


/*! \brief LTC_PKCS #1 v1.5 encode.
*
*  \param msg              The data to encode
*  \param msglen           The length of the data to encode (octets)
*  \param block_type       Block type to use in padding (\sa ltc_pkcs_1_v1_5_blocks)
*  \param modulus_bitlen   The bit length of the RSA modulus
*  \param prng             An active PRNG state (only for LTC_LTC_PKCS_1_EME)
*  \param prng_idx         The index of the PRNG desired (only for LTC_LTC_PKCS_1_EME)
*  \param out              [out] The destination for the encoded data
*  \param outlen           [in/out] The max size and resulting size of the encoded data
*
*  \return CRYPT_OK if successful
*/
int pkcs_1_v1_5_encode(const unsigned char *msg,
	unsigned int  msglen,
	unsigned  block_type,
	unsigned int  modulus_bitlen,
	unsigned char prng,
	unsigned  prng_idx,
	unsigned char *out,
	unsigned int *outlen)
{
	unsigned int modulus_len, ps_len, i;
	unsigned char *ps;
	int result;


	/* valid block_type? */
	if ((block_type != LTC_LTC_PKCS_1_EMSA) &&
		(block_type != LTC_LTC_PKCS_1_EME)) {
		return CRYPT_PK_INVALID_PADDING;
	}

	if (block_type == LTC_LTC_PKCS_1_EME) {    /* encryption padding, we need a valid PRNG */
		if ((result = prng_is_valid(prng_idx)) != CRYPT_OK) {
			return result;
		}
	}

	modulus_len = (modulus_bitlen >> 3) + (modulus_bitlen & 7 ? 1 : 0);

	/* test message size */
	if ((msglen + 11) > modulus_len) {
		return CRYPT_PK_INVALID_SIZE;
	}

	if (*outlen < modulus_len) {
		*outlen = modulus_len;
		result = CRYPT_BUFFER_OVERFLOW;
		goto bail;
	}

	/* generate an octets string PS */
	ps = &out[2];
	ps_len = modulus_len - msglen - 3;

	//  if (block_type == LTC_LTC_PKCS_1_EME) {
	//    /* now choose a random ps */
	//    if (prng_descriptor[prng_idx].read(ps, ps_len, prng) != ps_len) {
	//      result = CRYPT_ERROR_READPRNG;
	//      goto bail;
	//    }

	//    /* transform zero bytes (if any) to non-zero random bytes */
	//    for (i = 0; i < ps_len; i++) {
	//      while (ps[i] == 0) {
	//        if (prng_descriptor[prng_idx].read(&ps[i], 1, prng) != 1) {
	//          result = CRYPT_ERROR_READPRNG;
	//          goto bail;
	//        }
	//      }
	//    }
	//  } else {
	//    XMEMSET(ps, 0xFF, ps_len);
	//  }

	//上面代码是关于填充的，以自己的代码改写
	if (block_type == LTC_LTC_PKCS_1_EME)
	{
		//很假的随机数
		for (i = 0; i<ps_len; i++)
		{
			ps[i] = (unsigned char)i;
			ps[i] |= msg[0];

			if ((ps[i] & 0x01) == 0)
			{
				ps[i] = ~ps[i];
			}

			if (ps[i] == 0)
			{
				ps[i] = 0xA1;
			}
		}
	}
	else
	{
		memset(ps, 0xFF, ps_len);
	}




	/* create string of length modulus_len */
	out[0] = 0x00;
	out[1] = (unsigned char)block_type;  /* block_type 1 or 2 */
	out[2 + ps_len] = 0x00;
	memcpy(&out[2 + ps_len + 1], msg, msglen);
	*outlen = modulus_len;

	result = CRYPT_OK;
bail:
	return result;
} /* pkcs_1_v1_5_encode */













